# -*- coding: utf-8 -*-
"""
Created on Sun Sep  8 01:25:11 2024

@author: LENOVO
"""

# -*- coding: utf-8 -*-
"""
Created on Sat Sep  7 12:28:57 2024

@author: LENOVO
"""
import matplotlib.pyplot as plt
import numpy as np
from sympy import *
from scipy.optimize import root, fsolve
import pandas as pd
import math
from sympy import Symbol
from sympy.solvers import solve

H=1.7
a=16*H  #m
b=H/(2*np.pi) #m
vh=1 #m/s
L0=(341-27.5*2)/100
Lb=(220-27.5*2)/100

r1=lambda theta:((a-b*(theta)))
# r2=lambda theta:((a-b*(theta+np.pi)))

f1=lambda theta:((a-b*(theta)))-4.5
theta1=root(f1,0).x[0] #进入调头空间角度
#进入掉头空间时间
t1=(b*theta1-a)*np.sqrt((a-b*theta1)**2+b**2)/(2*b)-0.5*b*np.log(np.sqrt((a-b*theta1)**2+b**2)+a-b*theta1)+a/(2*b)*np.sqrt(a**2+b**2)+0.5*b*np.log(a+np.sqrt(a**2+b**2))
#进入掉头空间时 龙头位置

#某点斜率函数
def Slope(ta):
    k=(-a*np.cos(ta)+b*np.sin(ta)+b*ta*np.cos(ta))/(-a*np.sin(ta)-b*np.cos(ta)+b*ta*np.sin(ta))
    return k
# theta=np.linspace(0,32*np.pi,30*180)


# ax=plt.subplot(111, polar=True)
# ax.set_theta_direction(-1)
# plt.plot(theta,r(theta),lw=1,c='r', label='原始数据点')
# plt.legend()
# plt.title('非线性最小二乘拟合')
# plt.grid()  
# plt.show()




thetas=lambda t:(a-np.sqrt(a**2-2*b*vh*t))/b #猜测角度值

def theta(t):
    f=lambda x:(b*x-a)*np.sqrt((a-b*x)**2+b**2)/(2*b)-0.5*b*np.log(np.sqrt((a-b*x)**2+b**2)+a-b*x)+a/(2*b)*np.sqrt(a**2+b**2)+0.5*b*np.log(a+np.sqrt(a**2+b**2))-t
    theta=root(f,thetas(t))
    theta=theta.x[0]
    return theta


def position(i):
    
    r=r1
    Data=[]
    THETA=[]
    X=[]
    Y=[]
    # y=np.ones((301,1))
    
    THETA.append(theta(i))
    X.append(r(THETA[0])*np.cos((THETA[0])))
    Y.append(-r(THETA[0])*np.sin((THETA[0])))

    Data.append(theta(i))
    f=lambda thetai:(r(THETA[0]))**2+(r(thetai))**2-L0**2-2*(r(THETA[0]))*(r(thetai))*np.cos(THETA[0]-thetai)
    thetai=root(f,THETA[0]-0.5)
    thetai=thetai.x[0]
    
    # print("theta1:",thetai)
    THETA.append(thetai)
    X.append(r(THETA[1])*np.cos((THETA[1])))
    Y.append(-r(THETA[1])*np.sin((THETA[1])))
    Data.append(thetai)
    
    for j in range(0,222,1):
        
        f=lambda thetai:(r(THETA[1+j]))**2+(r(thetai))**2-Lb**2-2*(r(THETA[1+j]))*(r(thetai))*np.cos(THETA[1+j]-thetai)
        thetai=root(f,THETA[1+j]-0.5)
        thetai=thetai.x[0]
        THETA.append(thetai)
        X.append(r(THETA[j+2])*np.cos((THETA[j+2])))
        Y.append(-r(THETA[j+2])*np.sin((THETA[j+2])))

    A=np.column_stack((X, Y, THETA))

    return A

#第ts开始进入调头曲线,txs之前的全部龙头位置
def WhenWhereH(t,tx):
    theta0=theta(t)
    P0=position(t)
    x0=P0[0,0]
    y0=P0[0,1]
    k0=Slope(theta0)
    k1=-1/(y0/x0)
    tan=(k1-k0)/(1+k0*k1)
    fai=np.arctan(abs(tan))
    R=r1(theta0)/(3*np.cos(fai))
    # fo1=lambda x:[(y0-x[1])/(x0-x[0])+k0,(x0-x[0])**2+(y0-x[1])**2-(2*R)**2]
    # o1=fsolve(fo1,[x0-0.1,y0-0.1])
    # fo2=lambda x:[(-y0-x[1])/(-x0-x[0])+k0,(-x0-x[0])**2+(-y0-x[1])**2-(R)**2]
    # o2=fsolve(fo2,[-x0-0.1,-y0-0.1])
    # o=np.vstack((o1,o2)) #两个圆心原点坐标 第一个为2R 第二个为R
    s=3*R*(np.pi-2*fai)
    
    
    #大圆圆心坐标系下ABC三点坐标
    xA=-2*R*np.cos(fai)
    yA=2*R*np.sin(fai)
    xB=2*R*np.cos(fai)
    yB=2*R*np.sin(fai)
    xC=4*R*np.cos(fai)
    yC=2*R*np.sin(fai)
    
    #坐标变换矩阵 
    Z=np.array([[-x0/r1(theta0),y0/r1(theta0)],[-y0/r1(theta0),-x0/r1(theta0)]])
    #螺旋线到圆弧坐标
    def LTY(Y):
        m=(xA+xC)/2
        n=(yA+yC)/2
        M=np.array([m,n])
        L=Z@(Y-M.T)
        return L
    # return(LTY(np.array([xA,yA]))) #测试点
    
    #时间节点
    t0=t
    t1=2*R*(np.pi-2*fai)/vh+t0
    t2=R*(np.pi-2*fai)/vh+t1
    deltat=t2-t0
    #龙头位置：龙头在螺旋线坐标系下坐标 
    def Loonghead (t):
        #未进入圆弧前 
        if (t>=0 and t<t0):
            P=position(t)
            x=P[0,0]
            y=P[0,1]
            GPS=np.array([x,y])
        #第一段圆弧
        elif (t>=t0 and t<t1):
            x=2*R*np.cos(np.pi-fai-vh*(t-t0)/(2*R))
            y=2*R*np.sin(np.pi-fai-vh*(t-t0)/(2*R))
            ZB=np.array([x,y])
            GPS=LTY(ZB.T)
        #第二段圆弧    
        elif (t>=t1 and t<t2):
            x=R*np.cos(np.pi-fai-vh*(t-t1)/R)+3*R*np.cos(fai)
            y=-R*np.sin(np.pi-fai-vh*(t-t1)/R)+3*R*np.sin(fai)
            ZB=np.array([x,y])
            GPS=LTY(ZB.T)
        #出圆弧后 由中心对称可知
        else:
            P=position(t0+t2-t)
            x=-P[0,0]
            y=-P[0,1]
            GPS=np.array([x,y])
        return GPS
    LTQ=[]
    delta=0.1
    for i in np.linspace(0,tx,int(tx/delta)+1):
        LTQ.append(Loonghead(i))
    return ltq#测试点

LTQ=WhenWhereH(1332.782662,1332.782662+100)
LTQ1=pd.DataFrame(LTQ)
LTQ1.to_excel('tx.xlsx',index=False,header=False)


delta=0.1
Bodys=[]

XBS=[]
YBS=[]
for t in range(1,101,1):
    Body=[]
    XB=[]
    YB=[]
    body=[LTQ[len(LTQ)-int(1/delta*(100-t))-1][0],LTQ[len(LTQ)-int(1/delta*(100-t))-1][1]]
    XB.append(LTQ[len(LTQ)-int(1/delta*(100-t))-1][0])
    YB.append(LTQ[len(LTQ)-int(1/delta*(100-t))-1][1])
    Body.append(body)
    for i in range(len(LTQ)-int(1/delta*(100-t))-1,0,-1):
        if (((Body[0][0]-LTQ[i][0])**2+(Body[0][1]-LTQ[i][1])**2-L0**2)>=0):
            Body.append([LTQ[i][0],LTQ[i][1]])
            XB.append(LTQ[i][0])
            YB.append(LTQ[i][1])
            break
            # print(Body)
    for j in range(2,224,1):
        for k in range(i,0,-1):
            if (((Body[j-1][0]-LTQ[k][0])**2+(Body[j-1][1]-LTQ[k][1])**2-Lb**2)>=0):
                Body.append([LTQ[k][0],LTQ[k][1]])
                XB.append(LTQ[k][0])
                YB.append(LTQ[k][1])
                i=k
                break
            # else:
            #     continue
    Bodys.append(Body)
    XBS.append(XB)
    YBS.append(YB)
    df=pd.read_excel('tx.xlsx')
    a=df.values[:,0]
    b=y=df.values[:,1]
    VX=[]
    VY=[]
    V=[]
    def Speed(t):
        for i in range(0,224,1):
            vx=a[len(a)-10*(100-t)-i]-a[len(a)-10*(100-t)-i-10]
            vy=b[len(a)-10*(100-t)-i]-b[len(a)-10*(100-t)-i-10]
            v=np.sqrt(vx**2+vy**2)
            V.append(v)
        return V
    # SPEED=[]
    # for i in np.linspace(90,100,10):
    #     SPEED.append(Speed)
        
    # SPEED=pd.DataFrame(np.array(SPEED))
    # SPEED.to_excel('SPEED.xlsx',index=False,header=False)
    
        
# LTQ
# print(Bodys)
# Bodys=np.array(Bodys)
# XBS=np.array(XBS)
# YBS=np.array(YBS)
# XBS= XBS.transpose()
# YBS= YBS.transpose()
# BODYS=[]
# for i in range(0,224):
#     BODYS.append(XBS[i])
#     BODYS.append(YBS[i])

# XBS=pd.DataFrame(XBS)
# XBS.to_excel("XBS2.xlsx",index=False,header=False)
# Bodyexcel=pd.DataFrame(BODYS)
# Bodyexcel.to_excel("BODYS2.xlsx",index=False,header=False)
Bodys=np.array(Bodys)
# XBS=np.array(XBS)
# YBS=np.array(YBS)
# XBS= XBS.transpose()
# YBS= YBS.transpose()
# BODYS=[]
# for i in range(0,224):
#     BODYS.append(XBS[i])
#     BODYS.append(YBS[i])

# XBS=pd.DataFrame(XBS)
# XBS.to_excel("XBS2.xlsx",index=False,header=False)
Bodyexcel=pd.DataFrame(Bodys)
Bodyexcel.to_excel("qiusudu.xlsx",index=False,header=False)


def Velocity(i):
    P=position(i)
    V=[]
    V.append(1)
    for j in range(1,224):
        L=np.array([P[j-1,0]-P[j,0],P[j-1,1]-P[j,1]])
        H=np.array([1/np.sqrt(1**2+(Slope(P[j-1,2]))**2),Slope(P[j-1,2])/np.sqrt(1**2+Slope(P[j-1,2])**2)])
        T=np.array([1/np.sqrt(1**2+(Slope(P[j,2]))**2),Slope(P[j,2])/np.sqrt(1**2+Slope(P[j,2])**2)])
        # V1=np.array([1,H])
        # V2=np.array([v,T])
        f=lambda v:abs((V[j-1]*H[0]*L[0]+V[j-1]*H[1]*L[1]))-abs((v*T[0]*L[0]+v*T[1]*L[1]))
        v=root(f,V[j-1]-0.01)
        v=v.x[0]
        V.append(v)
    return V


# V=Velocity(200)


def Storage (i):
    p=position(i)
    speed=Velocity(i)
    Info=np.column_stack((p[:,0], p[:,1], speed))
    return Info

# #附件位置
# TWZ=[]
# for i in np.linspace(1332.782662-100,1332.782662,101):
#     S=Storage(i)
#     for j in range(0,224):       
#         TWZ.append(S[j,0])
#         TWZ.append(S[j,1])
# TWZ=np.array(TWZ)
# TWZ=TWZ.reshape(101,448)
# TWZ=TWZ.transpose()
# TWZ=pd.DataFrame(TWZ)
# TWZ.to_excel("第四问附件位置.xlsx",index=False,header=False)

#附件速度
# TV=[]
# for i in np.linspace(1332.782662-100,1332.782662,101):
#     S=Storage(i)
#     for j in range(0,224): 
#         TV.append(S[j,2])
# TV=np.array(TV)
# TV=TV.reshape(101,224)
# TV=TV.transpose()
# TV=pd.DataFrame(TV)
# TV.to_excel("第四问附件速度-100.xlsx",index=False,header=False)
                
    































            

#第ts开始进入调头曲线,txs之前的全部龙头位置
def Turn(t):
    theta0=theta(t)
    P0=position(t)
    x0=P0[0,0]
    y0=P0[0,1]
    k0=Slope(theta0)
    k1=-1/(y0/x0)
    tan=(k1-k0)/(1+k0*k1)
    fai=np.arctan(abs(tan))
    R=r1(theta0)/(3*np.cos(fai))
    # fo1=lambda x:[(y0-x[1])/(x0-x[0])+k0,(x0-x[0])**2+(y0-x[1])**2-(2*R)**2]
    # o1=fsolve(fo1,[x0-0.1,y0-0.1])
    # fo2=lambda x:[(-y0-x[1])/(-x0-x[0])+k0,(-x0-x[0])**2+(-y0-x[1])**2-(R)**2]
    # o2=fsolve(fo2,[-x0-0.1,-y0-0.1])
    # o=np.vstack((o1,o2)) #两个圆心原点坐标 第一个为2R 第二个为R
    s=3*R*(np.pi-2*fai)#弧长
    
    
    #大圆圆心坐标系下ABC三点坐标
    xA=-2*R*np.cos(fai)
    yA=2*R*np.sin(fai)
    xB=2*R*np.cos(fai)
    yB=2*R*np.sin(fai)
    xC=4*R*np.cos(fai)
    yC=2*R*np.sin(fai)
    
    #坐标变换矩阵 
    Z=np.array([[-x0/r1(theta0),y0/r1(theta0)],[-y0/r1(theta0),-x0/r1(theta0)]])
    #螺旋线到圆弧坐标
    def LTY(Y):
        m=(xA+xC)/2
        n=(yA+yC)/2
        M=np.array([m,n])
        L=Z@(Y-M.T)
        return L
    # return(LTY(np.array([xA,yA]))) #测试点
    
    # 时间节点
    t0=t
    t1=2*R*(np.pi-2*fai)/vh+t0
    t2=R*(np.pi-2*fai)/vh+t1
    deltat=t2-t0
    O2y=np.array([3*R*np.cos(fai),3*R*np.sin(fai)])
    O1=LTY(np.array([0,0]))
    O2=LTY(O2y)
    A=LTY(np.array([xA,yA]))
    B=LTY(np.array([xB,yB]))
    C=LTY(np.array([xC,yC]))

    O2c=np.array([3*R*np.cos(fai),3*R*np.sin(fai)-R])
    O1c=np.array([0,2*R])
    O2C=LTY(O2c)
    O1C=LTY(O1c)


    D1=[]
    tx=Symbol('tx')
    x=R*cos(np.pi-fai-vh*(tx-t1)/R)+3*R*cos(fai)
    y=-R*sin(np.pi-fai-vh*(tx-t1)/R)+3*R*sin(fai)
    func=solve((x-xC)**2+(y-yC)**2-L0**2)
    if(func[0]>t1 and func[0]<t2):
        J=np.array([(R*cos(np.pi-fai-vh*(func[0]-t1)/R)+3*R*cos(fai)),-R*sin(np.pi-fai-vh*(func[0]-t1)/R)+3*R*sin(fai)])
        # J=LTY(J)
    elif(func[1]>t1 and func[1]<t2):
        J=np.array([(R*cos(np.pi-fai-vh*(func[1]-t1)/R)+3*R*cos(fai)),-R*sin(np.pi-fai-vh*(func[1]-t1)/R)+3*R*sin(fai)])
        # J=LTY(J)
    else:
        J=np.array([xC,yC])
    D1.append(J)
    # return D1,func,t0,t1,t2
    d1=sqrt((J[0]-xB)**2+(J[1]-yB)**2)
    d2=sqrt((J[0]-xA)**2+(J[1]-yA)**2)
    while(d2>=Lb):
        if(d1>=Lb):
            x=R*cos(np.pi-fai-vh*(tx-t1)/R)+3*R*cos(fai)
            y=-R*sin(np.pi-fai-vh*(tx-t1)/R)+3*R*sin(fai)
            func=solve((x-J[0])**2+(y-J[1])**2-Lb**2)
            if(func[0]>t1 and func[0]<t2):
                J=np.array([(R*cos(np.pi-fai-vh*(func[0]-t1)/R)+3*R*cos(fai)),-R*sin(np.pi-fai-vh*(func[0]-t1)/R)+3*R*sin(fai)])
                D1.append(J)
                d1=np.sqrt((J[0]-xB)**2+(J[1]-yB)**2)
                d2=np.sqrt((J[0]-xA)**2+(J[1]-yA)**2)
                # J=LTY(J)
            elif(func[1]>t1 and func[1]<t2):
                J=np.array([(R*cos(np.pi-fai-vh*(func[1]-t1)/R)+3*R*cos(fai)),-R*sin(np.pi-fai-vh*(func[1]-t1)/R)+3*R*sin(fai)])
                D1.append(J)
                d1=np.sqrt((J[0]-xB)**2+(J[1]-yB)**2)
                d2=np.sqrt((J[0]-xA)**2+(J[1]-yA)**2)
                # J=LTY(J)
            else:
                x=2*R*cos(np.pi-fai-vh*(tx-t0)/(2*R))
                y=2*R*sin(np.pi-fai-vh*(tx-t0)/(2*R))
                func=solve((x-J[0])**2+(y-J[1])**2-Lb**2)
                if(func[0]>t0 and func[0]<t1):
                    J=np.array([[2*R*cos(np.pi-fai-vh*(func[0]-t0)/(2*R)),2*R*sin(np.pi-fai-vh*(func[0]-t0)/(2*R))]])
                    D1.append(J)
                    d1=0
                    d2=np.sqrt((J[0]-xA)**2+(J[1]-yA)**2)
                elif (func[1]>t0 and func[1]<t1):
                  
                        J=np.array([[2*R*cos(np.pi-fai-vh*(func[1]-t0)/(2*R)),2*R*sin(np.pi-fai-vh*(func[1]-t0)/(2*R))]])
                        D1.append(J)
                        d1=0
                        d2=np.sqrt((J[0]-xA)**2+(J[1]-yA)**2)
                else:
                    break
        else:
            if (d2>Lb):
                x=2*R*cos(np.pi-fai-vh*(tx-t0)/(2*R))
                y=2*R*sin(np.pi-fai-vh*(tx-t0)/(2*R))
                func=solve((x-J[0])**2+(y-J[1])**2-Lb**2)
                if(func[0]>t0 and func[0]<t1):
                    J=np.array([2*R*cos(np.pi-fai-vh*(func[0]-t0)/(2*R)),2*R*sin(np.pi-fai-vh*(func[0]-t0)/(2*R))])
                    D1.append(J)
                    d1=0
                    d2=sqrt((J[0]-xA)**2+(J[1]-yA)**2)
                elif (func[1]>t0 and func[1]<t1):
                        J=np.array([2*R*cos(np.pi-fai-vh*(func[1]-t0)/(2*R)),2*R*sin(np.pi-fai-vh*(func[1]-t0)/(2*R))])
                        D1.append(J)
                        d1=0
                        d2=np.sqrt((J[0]-xA)**2+(J[1]-yA)**2)
                else:
                    break
        D2=[]       
        for i in range(len(D1)):
            D2.append(LTY(D1[i]))
        # return D2
            
        

                
            
        
    
    


    # D=[]
    
    # d1=np.sqrt((C[0]-B[0])**2+(C[1]-B[1])**2)
    # d2=np.sqrt((C[0]-A[0])**2+(C[1]-A[1])**2)
    # x=Symbol('x')
    # y=Symbol('y')
   
    # if (d1>=L0):
    #     f1=[(x-O2[0])**2+(y-O2[1])**2-R**2,(x-C[0])**2+(y-C[1])**2-L0**2]
    #     J=solve(f1,(x,y))
    #     print(J)
    #     print(J[0])
    #     print(J[0][0])
    #     if(np.sqrt((J[0][0]-B[0])**2+(J[0][1]-B[1])**2)<=(J[1][0]-B[0])**2+(J[1][1]-B[1])**2):
    #         J=np.array([J[0][0],J[0][1]])
    #         d1=np.sqrt((J[0]-B[0])**2+(J[1]-B[1])**2)
    #         d2=np.sqrt((J[0]-A[0])**2+(J[1]-A[1])**2)
    #         D.append(J)
    #     else:
    #         J=np.array([J[1][0],J[1][1]])
    #         d1=np.sqrt((J[0]-B[0])**2+(J[1]-B[1])**2)
    #         d2=np.sqrt((J[0]-A[0])**2+(J[1]-A[1])**2)
    #         D.append(J)
    
        
    #     print(1)
           
    # else:
    #     if(d2>=L0):
    #         f1=[(x-O1[0])**2+(y-O1[1])**2-4*R**2,(x-C[0])**2+(y-C[1])**2-L0**2]
    #         J=solve(f1,(x,y))    
    #         D.append(J)
    #         d1=0
    #         d2=np.sqrt((J[0]-A[0])**2+(J[1]-A[1])**2)
    #         print(2)
    #     else:
    #         print("警告")
    #         # f1=lambda x:[x[0]-a+b*x[1],(x[0]*np.cos(x[1])-C[0])**2+(-x[0]*np.sin(x[1])-C[1])**2-L0**2]
    #         # J=fsolve(f1,[r1(90),90])

    # while(d2>=Lb):
    #     if (d1>=Lb):
    #         f1=lambda x:[(x[0]-O2[0])**2+(x[1]-O2[1])**2-R**2,(x[0]-J[0])**2+(x[1]-J[1])**2-Lb**2]
    #         J=fsolve(f1,[O2C[0],O2C[1]])
    #         D.append(J)
    #         d1=np.sqrt((J[0]-B[0])**2+(J[1]-B[1])**2)
    #         d2=np.sqrt((J[0]-A[0])**2+(J[1]-A[1])**2)
    #         print(3)
    #     else:
    #         if(d2>=Lb):
    #             f1=lambda x:[(x[0]-O1[0])**2+(x[1]-O1[1])**2-4*R**2,(x[0]-J[0])**2+(x[1]-J[1])**2-Lb**2]
    #             J=fsolve(f1,[O1C[0]-1,O1C[1]-1])
    #             D.append(J)
    #             d1=0
    #             d2=np.sqrt((J[0]-A[0])**2+(J[1]-A[1])**2)
    #             print(4)
    #         else:
    #             break
    # return D
#     # print(D)
    J=D2[1]   
    f1=lambda x:[x[0]-a+b*x[1],(x[0]*np.cos(x[1])-J[0])**2+(-x[0]*np.sin(x[1])-J[1])**2-Lb**2]
    J2=fsolve(f1,[r1(theta1)+0.01,theta1-0.01])
    # return J2

    THETA=[]
    X=[]
    Y=[]
    # y=np.ones((301,1))

    THETA.append(J2[1])
    X.append(r1(THETA[0])*np.cos((THETA[0])))
    Y.append(-r1(THETA[0])*np.sin((THETA[0])))

    f=lambda thetai:(r1(THETA[0]))**2+(r1(thetai))**2-L0**2-2*(r1(THETA[0]))*(r1(thetai))*np.cos(THETA[0]-thetai)
    thetai=root(f,THETA[0]-0.5)
    thetai=thetai.x[0]
    
    # print("theta1:",thetai)
    THETA.append(thetai)
    X.append(r1(THETA[1])*np.cos((THETA[1])))
    Y.append(-r1(THETA[1])*np.sin((THETA[1])))
    # Data.append(thetai)
    
    for j in range(0,224-len(D2)-1,1):
        
        f=lambda thetai:(r1(THETA[1+j]))**2+(r1(thetai))**2-Lb**2-2*(r1(THETA[1+j]))*(r1(thetai))*np.cos(THETA[1+j]-thetai)
        thetai=root(f,THETA[1+j]-0.5)
        thetai=thetai.x[0]
        THETA.append(thetai)
        X.append(r1(THETA[j+2])*np.cos((THETA[j+2])))
        Y.append(-r1(THETA[j+2])*np.sin((THETA[j+2])))
    A=np.column_stack((X, Y, THETA))
        # print(THETA)
    # return A
    xh0=27.5/100
    yh0=15/100
    P=A
    c=theta0%(2*np.pi)-np.pi
    # k0=(P[0,1]-P[1,1])/(P[0,0]-P[1,0])
    a1=1/(np.sqrt(1+k0**2)) #cos
    a2=k1/(np.sqrt(1+k0**2)) #sin
    a1=c*a1/abs(c)
    a2=c*a2/abs(c)
    xh=xh0*a1-yh0*a2+C[0]
    yh=xh0*a2+yh0*a1+C[1]
    H=np.array([xh,yh])
    # #Tail
    # xt0=-((341-27.5)/100)
    # yt0=15/100
    # xt=xt0*a1-yt0*a2+C[0]
    # yt=xt0*a2+yt0*a1+C[1]
    # T=np.array([xt,yt])


    dh=[]
    for i in range(0,222-len(D2)-1):
        k=(P[i,1]-P[i+1,1])/(P[i,0]-P[i+1,0])
        distance=abs(k*(H[0]-P[i,0])-H[1]+P[i,1])/np.sqrt(k**2+1)
        dh.append(distance)
        
    # dt=[]
    # for i in range(0,221):
    #     k=(P[i,1]-P[i+1,1])/(P[i,0]-P[i+1,0])
    #     distance=abs(k*(T[0]-P[i,0])-T[1]+P[i,1])/np.sqrt(k**2+1)
    #     dt.append(distance)   
    
    distance1=min(dh)
    # distance2=min(dt)
    sub=distance1-0.15
    #画图
    # WZ=A
    # X1=WZ[:,0]
    # Y1=WZ[:,1]
    # X2=-X1[::-1]
    # Y2=-Y1[::-1]
    # X=np.hstack((X1,X2))
    # Y=np.hstack((Y1,Y2))
    # plt.plot(X1,Y1,lw=1,c='b', label='入')
    # plt.plot(X2,Y2,lw=1,c='r', label='出')
    # plt.legend()
    # plt.title('全过程')
    # plt.grid()  
    # plt.show()
    
    return sub

# SUB=[]
# for i in np.linspace(t1,1335,500):
    
#    if(Turn(i)<=0):
#        print(i)
#        break
    
# i=np.linspace(t1,1335,500)
# plt.plot(i,SUB,lw=3,c='r', label='原始数据点')
# plt.legend()
# plt.title('非线性最小二乘拟合')
# plt.grid()  
# plt.show()

